#2 August 2011
#Duke University
#
#Jordan Besnoff
#
#This program will estimate the position and heading of a person walking among a grid of RFID tags using an antenna
#mounted on a cane via a least squares estimator. The location of each tag is assumed to be know prior, and each
#tag ID will be associated with a spatial position. After a read operation, a list of read tag ID's and the
#associated RSSI is available, and this information will be used to perform the estimation.


import sys, time, string, lib_M5e_VA, sets
from numpy import *
from scipy import *
from scipy.optimize import fmin   #NECESSARY FOR FUNCTION MINIMIZATION
from Tkinter import *
import math as mth
import matplotlib.pyplot as plt   #FOR ANIMATED PLOTS

#------------------------------Load grid map----------------------------------------------#
f = open('/Users/Jordan/Dropbox/Duke/Research/Cane_antenna/Python_files/grid_map.txt','r')
tlist = []

for line in f:
    a = line
    a = a.split()
    tlist.append(a)

grid = array(tlist)

#REFORMAT TAG IDS IN STORED MAP
replace = []
stored = grid[:,0]
for x in stored:
    tmp = []
    for i in range(0,len(x),4):
        tmp.append(chr(int(x[i:(i+4)],16)))
    s = ''.join(tmp)
    replace.append(s)

grid[:,0] = replace
#------------------------------------------------------------------------------------------#

#--------Log file----------------------#
global logger
logger = open('tag_log.txt','a')
#--------------------------------------#


#Vector normalization function (must send an ARRAY)
def unit_vec(v):
    if all(v) == 0:
        return v
    else:
        return v / sqrt(dot(v,v))



## Initialize reader
def P1(M5e):
    M5e.ChangeAntennaPorts(1,1)
    return 'AntPort1'

def P2(M5e):
    M5e.ChangeAntennaPorts(2,2)
    return 'AntPort2'

##def PrintDatum(data):
##    ant, ids, rssi = data
##    print data


def Store(stuff):
    global ant,ids,rssi,data_line,tag_reads
    ant, ids, rssi = stuff
##    print stuff

    #Create "line" of data and append
    data_line = [ids,rssi,time.asctime(time.localtime()),repr(time.time()),ant]
##    data_line = [ids,rssi]
    tag_reads.append(data_line)

    


    
#---------------Reader parameters-----------------#
Power = 3000
read_time = 0.600
#-------------------------------------------------#

#---------------------Model parameters---------------------------#
RF_out = Power/100   #RF output power, dBm
Gt = 3   #Transmitter antenna gain, dBi
Gr = -10   #Receiver gain, dBi
BS_loss = 20   #Backscatter loss, dB
freq = 915   #Frequency of operation, MHz
anth = 0.7366   #Height of reader above ground, approx 1 meter
#----------------------------------------------------------------#

#-----------------RSSI Conversion parameters------------------#
RSSI_slope = 0.75   #based on M5e model, change in RSSI per dB
#RSSI_norm = 86.6   #RSSI with tag at RSSI_dist, normalization
RSSI_norm = 90
#RSSI_dist = 1.04    #distance used for RSSI normalization, m
RSSI_dist = 0.762
#-------------------------------------------------------------#

cal_file = open('/Users/Jordan/Dropbox/Duke/Research/Cane_antenna/Python_files/cal.txt','r+')

flag = True
revert = False
#calibration time (in seconds)
cal_time = 5

while flag:
    cal = raw_input("Is calibration needed? [Y/N]: ")

    if cal == "N":
        print "\nCalibration is not needed. Reverting to saved calibration.\n"
        RSSI_norm = float(cal_file.readline().split()[0])
        RSSI_dist = float(cal_file.readline().split()[0])
        cal_file.close()
        
        flag = False

    elif cal == "Y":
        print "\nCalibration will utilize a single tag at a specified distance\n"
        cal_tag = raw_input("Enter ID of calibration tag: ")
        RSSI_dist = float(raw_input("Enter calibration distance in meters: "))

        print "\nBeginning calibration. Wait for completion before moving antenna"
        print "Initializing reader"
        r = lib_M5e_VA.M5e(readPwr=Power)
        print "Reader initialized, beginning calibration."

        tag_reads = []

        q = lib_M5e_VA.M5e_Poller(r, antfuncs=[P1], callbacks=[Store])

        q.query_mode()

        t0 = time.time()
        while time.time() - t0 < cal_time:
            blah = 0

        q.stop()

        if tag_reads == []:
            print "\nNo tags found! Reverting to stored calibration values."

            RSSI_norm = float(cal_file.readline().split()[0])
            RSSI_dist = float(cal_file.readline().split()[0])
            cal_file.close()

        else:
            #Reformat calibration tag
            tmp = []
            for i in range(0,len(cal_tag),4):
                tmp.append(chr(int(cal_tag[i:(i+4)],16)))
            cal_tag = ''.join(tmp)
            
            #Remove non-calibration tags
            tag_reads = array(tag_reads)
            
            rem = []
            for x in range(0,len(tag_reads)):

                if tag_reads[x,0] != cal_tag:
                    rem.append(x)

            tag_reads = delete(tag_reads,rem,0)

            #If calibration tag not found (only "spurious" tags)
            if len(tag_reads) == 0:
                print "\nCalibration tag not found! Reverting to stored calibration values."

                RSSI_norm = float(cal_file.readline().split()[0])
                RSSI_dist = float(cal_file.readline().split()[0])

                cal_file.close()

            else:  #Calibration tag was found, average the RSSI values
                #Show tag reads
                print tag_reads

                #Extract and average the RSSI values
                RSSI_val = []
                for x in tag_reads[:,1]:
                    RSSI_val.append(float(x))

                RSSI_val = array(RSSI_val)

                RSSI_norm = sum(RSSI_val) / len(RSSI_val)

                print "\nThe average RSSI value is:",RSSI_norm

                #Store calibration values
                cal_file.write(str(RSSI_norm)+'\n')
                cal_file.write(str(RSSI_dist))
                cal_file.close()

        flag = False

    else:
        print "\nInvalid selection, choose again.\n"


#--------------Load RSSI Map----------------------------#
global rssi_use
rssi_use = False

if rssi_use:
    rssi_map_file = open('rssi_map_file.txt','r')
    cont_flag = True
    tag_flag = True
    loc_flag = False
    global rssi_map_tags, rssi_map_loc

    rssi_map_tags = []
    rssi_map_loc = []
    grouping = []

    while(cont_flag):
        line = rssi_map_file.readline()
        line = line.strip()
        line = line.split()

        if line == ['-']:
            loc_flag = True
            tag_flag = False

        elif line == ['--']:
            loc_flag = False
            tag_flag = True

        elif line == []:
            cont_flag = False

        elif loc_flag:
            rssi_map_loc.append(line)
            grouping = array(grouping)
            rssi_map_tags.append(grouping)
            grouping = []

        elif tag_flag:
    ##        rssi_map_tags.append(line)
            grouping.append(line)

    rssi_map_file.close()
#------------------------------------------------------#
        

print "Initializing reader"
r = lib_M5e_VA.M5e(readPwr=Power)
##q = lib_M5e_VA.M5e_Poller(r, antfuncs=[P1], callbacks=[Store])

print "Reader initialized, beginning tag search"


##global heading_ready
##global old_est
heading_ready = False
old_est = []




def read():
    #Need this to clear tag reads each time?
    global tag_reads,heading_ready,old_est,xpos,ypos,old_read_time,old_WA_est,old_rssi_est
    global position,w_avg_est,rssi_map_est
    tag_reads = []

    q = lib_M5e_VA.M5e_Poller(r, antfuncs=[P1,P2], callbacks=[Store])

    q.query_mode()

    t0 = time.time()
    while time.time() - t0 < read_time:
        blah = 0

    q.stop()

    #Only prune the tag reads if valid reads exist
    if tag_reads != []:

##        print tag_reads
##        time.sleep(3)

        #Create array for the tag reads (for easier processing)
        tag_reads = array(tag_reads)
##        print tag_reads
##        time.sleep(3)

        #Remove redundant tag reads
        remove = []
        avg = []
        comp = tag_reads[:,0]

        for i in range(0,(len(comp)-1)):
            avg.append(float(tag_reads[i,1]))
            for j in range((i+1),len(comp)):
                if comp[i] == comp[j]:
                    remove.append(j)
                    avg.append(float(tag_reads[j,1]))
            #Average multiple reads (should rid of outliers?)
            avg_rep = sum(avg) / len(avg)
            tag_reads[i,1] = str(avg_rep)
            avg = []

        remove = list(set(remove))

        tag_reads = delete(tag_reads,remove,0)


        #Determine position of each tag read based on stored map
        xval = []
        yval = []
        xval_est = []
        yval_est = []
        for x in tag_reads[:,0]:
            ind = transpose(nonzero(grid == x))

            if any(ind):   #in case there are any extraneous reads, ignore them for processing

                xval.append([float(grid[ind[0,0],1])])
                yval.append([float(grid[ind[0,0],2])])

                xval_est.append(float(grid[ind[0,0],1]))
                yval_est.append(float(grid[ind[0,0],2]))

            else:
                tmp = transpose(nonzero(tag_reads == x))
                tag_reads = delete(tag_reads,tmp[0,0],0)

        xval_est = array(xval_est)
        yval_est = array(yval_est)

        #Append x and y position values, based on map, to tag reads (only for valid tag reads)
        if len(tag_reads) != 0:
            tag_reads = append(tag_reads,xval,axis=1)
            tag_reads = append(tag_reads,yval,axis=1)

            #Create array of read RSSI values (needed for estimation)
            rssi_mes = []
            for x in tag_reads[:,1]:
                rssi_mes.append(float(x))

            rssi_mes = array(rssi_mes)

            #Write valid tag reads to file
            global logger
            for x in range(0,len(tag_reads)):
##                if len(tag_reads[x,0]) != 12:
##                    logger.write("short id: "+'\n')
##                    print '-----------------------------------------'
##                    print tag_reads[x,0].encode('hex')
                logger.write(tag_reads[x,0].encode('hex')+','+tag_reads[x,1]+','+tag_reads[x,5]+','+tag_reads[x,6]+','+tag_reads[x,2]+','+repr(tag_reads[x,3])+','+tag_reads[x,4]+'\n')

            logger.write('-'+'\n')
            logger.flush()

        ##    #Function to minimize for estimation
        ##    def MSE(x):
        ##        return sum( (rssi_est - (RF_out + 2*Gt - 2*( 20*log10( sqrt( (x[0]-xval_est)**2 + (x[1]-yval_est)**2 + anth**2) ) + 20*log10(freq) - 27.55) - BS_loss))**2 )

            #Weighted average algorithm
            weights = rssi_mes/max(rssi_mes)
            weights = weights/sum(weights)
            

            #TEST: Function to minimize for estimation
            def MSE(x):
                return sum( (rssi_mes - (RSSI_norm + RSSI_slope*( 20*log10(2*RSSI_dist) - 20*log10( 2*sqrt( (x[0]-xval_est)**2 + (x[1]-yval_est)**2 + anth**2) ) ) ) )**2 ) 

            #-----------------RSSI map usage:-------------------------------#
            global rssi_use, rssi_map_tags, rssi_map_loc, rssi_comp,tr_comp,td
            global map_est_x,map_est_y, true_vals
            rssi_comp = []
            
            if rssi_use:
                tag_reads_tmp = sorted(tag_reads, key = lambda tag: tag[0])
                tr_comp = []
                for x in range(0,len(tag_reads)):
                    tval = tag_reads_tmp[x]
                    add = [tval[0].encode('hex'),tval[1]]
                    tr_comp.append(add)
                    
##                #Place read tags in correct format
##                tr_comp = []
##                for x in range(0,len(tag_reads)):
##                    add = [tag_reads[x,0].encode('hex'),tag_reads[x,1]]
##                    tr_comp.append(add)

                tr_comp = array(tr_comp)
                
                for x in range(0,len(rssi_map_tags)):
                    td = rssi_map_tags[x]
                    if len(tr_comp) == len(td):
                        if set(tr_comp[:,0]).issubset(set(td[:,0])):
                            #Determine "closeness"
                            tr_comp_vals = []
                            td_vals = []
                            for x in range(0,len(tr_comp)):
                                tr_comp_vals.append(float(tr_comp[x,1]))
                                td_vals.append(float(td[x,1]))
                            tr_comp_vals=array(tr_comp_vals)
                            td_vals=array(td_vals)
                            
                            comp_data = [sum((tr_comp_vals-td_vals)**2),x]
                            rssi_comp.append(comp_data)

                #If there are no perfect matches, look for subsets
                if rssi_comp == []:
                    for x in range(0,len(rssi_map_tags)):
                        td = rssi_map_tags[x]

                        if set(tr_comp[:,0]).issubset(set(td[:,0])):
                            #Determine "closeness"
                            tr_comp_vals = []
                            td_vals = []
                            for x in range(0,len(tr_comp)):
                                tr_comp_vals.append(float(tr_comp[x,1]))
                                td_vals.append(float(td[x,1]))
                            tr_comp_vals=array(tr_comp_vals)
                            td_vals=array(td_vals)
                            
                            comp_data = [sum((tr_comp_vals-td_vals)**2),x,abs(len(tr_comp)-len(td))]
                            rssi_comp.append(comp_data)

                    #Sort subset matches by difference in tag number (ascending)
                    rssi_comp = array(rssi_comp)
                    rssi_comp = sorted(rssi_comp, key = lambda tmp: tmp[2])

                    #Grab top 3 matches from subsets, based on size of subset compared to training data
                    if len(rssi_comp) > 3:
                        matches = 3
                        rssi_data = rssi_comp[0:matches]
                    else:
                        rssi_data = rssi_comp



                #If perfect matches exist, sort by closeness to RSSI values, take top 3 if there are that many
                else:
                    #Sort by closeness to RSSI values
                    rssi_comp = array(rssi_comp)
                    rssi_comp = sorted(rssi_comp, key = lambda tmp: tmp[0])

                    if len(rssi_comp) > 3:
                        matches = 3
                        rssi_data = rssi_comp[0:matches]
                    else:
                        rssi_data = rssi_comp

                #Estimation
                map_est_x = []
                map_est_y = []
                for x in range(0,len(rssi_data)):
                    tmp = rssi_data[x]
                    true_vals = rssi_map_loc[int(tmp[1])]
                    map_est_x.append(float(true_vals[0]))
                    map_est_y.append(float(true_vals[1]))
                        
                #"Multiple Nearest Neighbors," average out training data that is close
                if len(rssi_data) == 0:
                    print "NO MATCH!!!!!!!"
                    rssi_map_est = array([0,0])

                else:
                    rssi_map_est = array([sum(map_est_x)/len(map_est_x),sum(map_est_y)/len(map_est_y)])

            else:   #Placeholder when not using RSSI map
                rssi_map_est = array([0,0])
                
            #---------------------------------------------------------------#
                    
                    

            #Perform position estimation
            if len(tag_reads) == 1:
                position = array([float(tag_reads[0,5]),float(tag_reads[0,6])])
                w_avg_est = position
            else:
                x = [0,0]   #initial guess (need to start somewhere)
                position = fmin(MSE,x,disp=0)

                #Weighted average estimate for 2+ tags
                w_avg_x = sum(weights*xval_est)
                w_avg_y = sum(weights*yval_est)
                w_avg_est = array([w_avg_x,w_avg_y])

            #DEBUGGING
##            print tag_reads
##            print position
##            raw_input('Press Enter to continue...')


            #Heading estimate
            if heading_ready:
                heading = unit_vec(position - old_est)
                WA_heading = unit_vec(w_avg_est - old_WA_est)
                RSSI_heading = unit_vec(rssi_map_est - old_rssi_est)

                old_est = position
                old_WA_est = w_avg_est
                old_rssi_est = rssi_map_est

            else:
                old_est = position
                old_WA_est = w_avg_est
                old_rssi_est = rssi_map_est

            #Display results
##            print "\rx position:" + str(position[0])
##            print "\ry position:" + str(position[1])

            if heading_ready:
##                print "\rx heading:" + str(heading[0])
##                print "\ry heading:" + str(heading[1])

                xheading.set(str(heading[0]))
                yheading.set(str(heading[1]))
                xheading_WA.set(str(WA_heading[0]))
                yheading_WA.set(str(WA_heading[1]))
                xheading_rssi.set(str(RSSI_heading[0]))
                yheading_rssi.set(str(RSSI_heading[1]))

                new_xhead = [0,heading[0]]
                new_yhead = [0,heading[1]]
                compass.set_xdata(new_xhead)
                compass.set_ydata(new_yhead)
                plt.figure(2)
                plt.draw()

                #Heading vector on grid map
                arrow_len = 1
                angle = mth.atan2(heading[1],heading[0])
                angle_WA = mth.atan2(WA_heading[1],WA_heading[0])
                angle_RSSI = mth.atan2(RSSI_heading[1],RSSI_heading[0])

                if heading[0] == 0 and heading[1] == 0:
                    grid_xhead = 0
                    grid_yhead = 0
                else:
                    grid_xhead = arrow_len * mth.cos(angle)
                    grid_yhead = arrow_len * mth.sin(angle)

                if WA_heading[0] == 0 and WA_heading[1] == 0:
                    grid_xhead_WA = 0
                    grid_yhead_WA = 0
                else:
                    grid_xhead_WA = arrow_len * mth.cos(angle_WA)
                    grid_yhead_WA = arrow_len * mth.sin(angle_WA)

                if RSSI_heading[0] == 0 and RSSI_heading[1] == 0:
                    grid_xhead_rssi = 0
                    grid_yhead_rssi = 0

                else:
                    grid_xhead_rssi = arrow_len * mth.cos(angle_RSSI)
                    grid_yhead_rssi = arrow_len * mth.sin(angle_RSSI)
                    
                est_head.set_xdata([position[0],(position[0]+grid_xhead)])
                est_head.set_ydata([position[1],(position[1]+grid_yhead)])
                WA_head.set_xdata([w_avg_est[0],(w_avg_est[0]+grid_xhead_WA)])
                WA_head.set_ydata([w_avg_est[1],(w_avg_est[1]+grid_yhead_WA)])
                RSSI_head.set_xdata([rssi_map_est[0],(rssi_map_est[0]+grid_xhead_rssi)])
                RSSI_head.set_ydata([rssi_map_est[1],(rssi_map_est[1]+grid_yhead_rssi)])
                plt.figure(1)
                plt.draw()

##            else:
##                print "\rx heading:" + "0"
##                print "\ry heading:" + "0"

            if heading_ready:
                new_read_time = time.time()
                ud_time = new_read_time - old_read_time

                ud_rate.set(str(ud_time))

                old_read_time = new_read_time

            else:
                old_read_time = time.time()

            xpos.set(str(position[0]))
            ypos.set(str(position[1]))

            xpos_wa.set(str(w_avg_est[0]))
            ypos_wa.set(str(w_avg_est[1]))

            xpos_rssi.set(str(rssi_map_est[0]))
            ypos_rssi.set(str(rssi_map_est[1]))

            num_tags.set(str(len(tag_reads)))

            point.set_xdata(position[0])
            point.set_ydata(position[1])

            w_avg_point.set_xdata(w_avg_est[0])
            w_avg_point.set_ydata(w_avg_est[1])

            rssi_map_point.set_xdata(rssi_map_est[0])
            rssi_map_point.set_ydata(rssi_map_est[1])

            #Change colors of read tags
##            grid_tags.set_xdata(xtags)
##            grid_tags.set_ydata(ytags)

            xchange = []
            ychange = []
            for x in range(0,len(tag_reads)):
                xchange.append(float(tag_reads[x,5]))
                ychange.append(float(tag_reads[x,6]))

            plt.figure(1)
            tag_clr.set_xdata(xchange)
            tag_clr.set_ydata(ychange)
##            plt.plot(xchange,ychange,'ro')
            
            plt.draw()

            heading_ready = True

    root.after(100,read)

            
            #return heading_ready, old_est
#Extract all tag locations from grid to plot
xtags = []
ytags = []

for x in range(0,len(grid)):
    xtags.append(float(grid[x,1]))
    ytags.append(float(grid[x,2]))

#Turn on interactive mode (for plotting)
plt.ion()
#plt.subplot(121)
plt.figure(1)
point, = plt.plot(0,'r*',markersize=15)
w_avg_point, = plt.plot(0,'g*',markersize=15)
rssi_map_point, = plt.plot(0,'b*',markersize=15)
est_head, = plt.plot([0,0],[0,0],linewidth=2.0)
WA_head, = plt.plot([0,0],[0,0],linewidth=2.0)
RSSI_head, = plt.plot([0,0],[0,0],linewidth=2.0)
grid_tags, = plt.plot(xtags,ytags,'wo')
tag_clr, = plt.plot(-100,-100,'ro')
plt.title('Cane Position and Direction Estimate')
plt.grid(True)
xmin = min(xtags) - 3
xmax = max(xtags) + 3
ymin = min(ytags) - 3
ymax = max(ytags) + 3

plt.axis([xmin,xmax,ymin,ymax])

#plt.subplot(122)
plt.figure(2)
plt.title('Heading')
plt.text(0,1.25,'N')
plt.text(1.25,0,'E')
plt.text(0,-1.25,'S')
plt.text(-1.25,0,'W')
compass, = plt.plot([0,0],[0,0],linewidth=2.0)
plt.axis([-1.5,1.5,-1.5,1.5])

#TESTING: GUI OUTPUT
root = Tk()
root.title("Position Estimate")

xpos = StringVar()
ypos = StringVar()
xpos_wa = StringVar()
ypos_wa = StringVar()
xpos_rssi = StringVar()
ypos_rssi = StringVar()
xheading = StringVar()
yheading = StringVar()
ud_rate = StringVar()
num_tags = StringVar()
xheading_WA = StringVar()
yheading_WA = StringVar()
xheading_rssi = StringVar()
yheading_rssi = StringVar()

##Label(root,textvariable=xpos,width=100).pack()

def dump_button():
    global position,w_avg_est,rssi_map_est
    print xentry.get()
    print yentry.get()
##    accuracy.write('Actual location: '+'('+xentry.get()+','+yentry.get()+')'+'\n')
##    accuracy.write('Least Squares: '+'('+str(position[0])+','+str(position[1])+')'+'\n')
##    accuracy.write('Weighted Average: '+'('+str(w_avg_est[0])+','+str(w_avg_est[1])+')'+'\n')
##    accuracy.write('RSSI Map: '+'('+str(rssi_map_est[0])+','+str(rssi_map_est[1])+')'+'\n'+'\n')
    accuracy_actual.write(xentry.get()+' '+yentry.get()+'\n')
    accuracy_WA.write(str(w_avg_est[0])+' '+str(w_avg_est[1])+'\n')
    
def rssi_map_button():
    global tag_reads
    tag_reads_sort = sorted(tag_reads, key = lambda tag: tag[0])
    for x in range(0,len(tag_reads_sort)):
        tmp_write = tag_reads_sort[x]
        rssi_map_file.write(tmp_write[0].encode('hex')+' '+tmp_write[1]+'\n')
##        rssi_map_file.write(tag_reads[x,0].encode('hex')+','+tag_reads[x,1]+','+tag_reads[x,5]+','+tag_reads[x,6]+','+tag_reads[x,2]+','+repr(tag_reads[x,3])+','+tag_reads[x,4]+'\n')
    rssi_map_file.write('-'+'\n'+xentry.get()+' '+yentry.get()+' '+orientation_entry.get()+'\n'+'--'+'\n')
        

##accuracy = open('accuracy.txt','a')
accuracy_actual = open('accuracy_actual.txt','w')
accuracy_WA = open('accuracy_WA.txt','w')
if not(rssi_use):
    rssi_map_file = open('rssi_map_file.txt','w')

dump = Button(root, command = dump_button, text="Dump Data")
dump.grid(row=7,column=2)
rssi_map = Button(root, command = rssi_map_button, text="RSSI Map")
rssi_map.grid(row=8,column=2)

txpos = Label(root,text="True x position:")
txpos.grid(row=7)
typos = Label(root,text="True y position:")
typos.grid(row=8)
orientation = Label(root,text="Orientation:")
orientation.grid(row=9)

xentry = Entry(root)
xentry.grid(row=7,column=1)
yentry = Entry(root)
yentry.grid(row=8,column=1)
orientation_entry = Entry(root)
orientation_entry.grid(row=9,column=1)

xposlabel = Label(root,text="x position:")
yposlabel = Label(root,text="y position:")
xpos_wa_label = Label(root,text="x pos WA:")
ypos_wa_label = Label(root,text="y pos WA:")
xpos_map_label = Label(root,text="x pos RSSI map:")
ypos_map_label = Label(root,text="y pos RSSI map:")
update_rate = Label(root,text="Update rate:")
num_tags_lbl = Label(root,text="Number tag reads:")
xpos_wa_label.grid(row=2,sticky=W)
ypos_wa_label.grid(row=3,sticky=W)
xposlabel.grid(row=0,sticky=W)
yposlabel.grid(row=1,sticky=W)
update_rate.grid(row=6,sticky=W)
num_tags_lbl.grid(row=6,column=2,sticky=W)
xpos_map_label.grid(row=4,sticky=W)
ypos_map_label.grid(row=5,sticky=W)


xheadinglabel = Label(root,text="x heading:")
yheadinglabel = Label(root,text="y heading:")
xheadinglabel.grid(row=0,column=2)
yheadinglabel.grid(row=1,column=2)
xheading_WA_label = Label(root,text="x heading WA:")
yheading_WA_label = Label(root,text="y heading WA:")
xheading_WA_label.grid(row=2,column=2)
yheading_WA_label.grid(row=3,column=2)
xheading_map_label = Label(root,text="x heading RSSI map:")
yheading_map_label = Label(root,text="y heading RSSI map:")
xheading_map_label.grid(row=4,column=2)
yheading_map_label.grid(row=5,column=2)

xposmsg = Label(root,textvariable=xpos,width=20)
yposmsg = Label(root,textvariable=ypos,width=20)
xposmsg.grid(row=0,column=1)
yposmsg.grid(row=1,column=1)
xpos_wa_msg = Label(root,textvariable=xpos_wa,width=20)
ypos_wa_msg = Label(root,textvariable=ypos_wa,width=20)
xpos_wa_msg.grid(row=2,column=1)
ypos_wa_msg.grid(row=3,column=1)
xpos_map_msg = Label(root,textvariable=xpos_rssi,width=20)
ypos_map_msg = Label(root,textvariable=ypos_rssi,width=20)
xpos_map_msg.grid(row=4,column=1)
ypos_map_msg.grid(row=5,column=1)

xheadingmsg = Label(root,textvariable=xheading,width=20)
yheadingmsg = Label(root,textvariable=yheading,width=20)
xheadingmsg.grid(row=0,column=3)
yheadingmsg.grid(row=1,column=3)
xheadingWAmsg = Label(root,textvariable=xheading_WA,width=20)
yheadingWAmsg = Label(root,textvariable=yheading_WA,width=20)
xheadingWAmsg.grid(row=2,column=3)
yheadingWAmsg.grid(row=3,column=3)
xheading_map_msg = Label(root,textvariable=xheading_rssi,width=20)
yheading_map_msg = Label(root,textvariable=yheading_rssi,width=20)
xheading_map_msg.grid(row=4,column=3)
yheading_map_msg.grid(row=5,column=3)

ud_ratemsg = Label(root,textvariable=ud_rate,width = 20)
ud_ratemsg.grid(row=6,column=1)

num_tagsmsg = Label(root,textvariable=num_tags,width=20)
num_tagsmsg.grid(row=6,column=3)

#Set heading to (0,0) initially
xheading.set("0")
yheading.set("0")
xheading_WA.set("0")
yheading_WA.set("0")
xheading_rssi.set("0")
yheading_rssi.set("0")

#root.after(100,read(heading_ready,old_est))
read()
root.mainloop()


##Program cleanup?
plt.close()
plt.close()
logger.close()
cal_file.close()
f.close()
##accuracy.close()            
rssi_map_file.close()
accuracy_actual.close()
accuracy_WA.close()
